/*******       DTW.C         ********/
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "dmac.h"
#include "ADC.h"
#include "VAD.h"
#include "MFCC.h"
#include "DTW.h"

/*
 *  DTW算法 通过局部优化的方法实现加权距离总和最小

    时间规整函数：
    C={c(1),c(2),…,c(N)}
    N为路径长度，c(n)=(i(n),j(n))表示第n个匹配点是有参考模板的
    第i(n)个特征矢量与待测模板的第j(n)个特征矢量构成的匹配点对，两
    者之间的距离d(x(i(n)),y(j(n)))称为匹配距离。
    时间规整函数满足一下约束：
    1.单调性，规整函数单调增加。
    2.起点终点约束，起点对起点，终点对终点。
    3.连续性，不允许跳过任何一点。
    4.最大规整量不超过某一极限值。|i(n)-j(n)|<M,M为窗宽。规整
    函数所处的区域位于平行四边形内。局部路径约束，用于限制当第n步
    为(i(n),j(n))时，前几步存在几种可能的路径。

    DTW步骤：
    1.初始化。令i(0)=j(0)=0,i(N)=in_frm_num,j(N)=mdl_frm_num.
    确定一个平行四边形，有两个位于(0,0)和(in_frm_num,mdl_frm_num)的顶点，相邻斜边斜
    率分别是2和1/2.规整函数不可超出此平行四边形。
    2.递推求累计距离。

    若输入特征与特征模板的帧数差别过大，直接将匹配距离设为最大
    frm_in_num<(frm_mdl_num/2)||frm_in_num>(frm_mdl_num*2)
*/
int64_t avr_in[mfcc_num];
int64_t avr_mdl[mfcc_num];

int64_t standard_in[mfcc_num];
int64_t standard_mdl[mfcc_num];
// static int dtw_data[vv_frm_max * vv_frm_max];

int *pdtw_data = NULL;

struct pointOritation //节点方向，用来回溯每个W点
{
    int frontI, frontJ;
};
int g[vv_frm_max][vv_frm_max];
struct pointOritation pOritation[vv_frm_max][vv_frm_max]; //用来存放

void gArray(int *p, int n, int m, int *g, struct pointOritation *pr)
{
    int i = 0, j = 0;

    *g = (*p) * 2; //起始点（最左上角的点）
    for (i = 1; i < m; i++)
    { //最上面一横
        *(g + i) = *(g + i - 1) + *(p + i);
        (pr + i)->frontI = 0;
        (pr + i)->frontJ = i - 1;
    }

    for (i = 1; i < n; i++)
    { //最左边的一竖
        *(g + i * m + 0) = *(g + (i - 1) * m + 0) + (*(p + i * m + 0));
        (pr + i * m + 0)->frontI = i - 1;
        (pr + i * m + 0)->frontJ = 0;
    }

    //计算剩余网格的G值
    for (i = 1; i < n; i++)
    {
        for (j = 1; j < m; j++)
        {
            int left, up, incline;

            left = *(g + i * m + j - 1) + *(p + i * m + j);
            up = *(g + (i - 1) * m + j) + *(p + i * m + j);
            incline = *(g + (i - 1) * m + j - 1) + (*(p + i * m + j)) * 2;

            //从左、上、斜三个方向选出最小的
            int min = left;

            *(g + i * m + j) = min;
            (pr + i * m + j)->frontI = i;
            (pr + i * m + j)->frontJ = j - 1;

            if (min > up)
            {
                min = up;
                *(g + i * m + j) = min;
                (pr + i * m + j)->frontI = i - 1;
                (pr + i * m + j)->frontJ = j;
            }
            if (min > incline)
            {
                min = incline;
                *(g + i * m + j) = min;
                (pr + i * m + j)->frontI = i - 1;
                (pr + i * m + j)->frontJ = j - 1;
            }
        }
    }
#if 0
    //输出G数组
    for (i = 0; i < n; i++) {
        for (j = 0; j < m; j++)
            printf("%d, ", *(g+i*m+j));
        printf("\n");
    }
    //输出方向数组
    for (i = 0; i < n; i++) {
        for (j = 0; j < m; j++)
            printf("(%d,%d), ", (pr+i*m+j)->frontI, (pr+i*m+j)->frontJ);
        printf("\n");
    }
#endif
}
int printPath(struct pointOritation *po, int n, int m, int *g)
{
    //从最后一个点向前输出路径节点
    int i = n - 1, j = m - 1;
    int step = 0;

    while (1)
    {
        int ii = (po + i * m + j)->frontI, jj = (po + i * m + j)->frontJ;

        if (i == 0 && j == 0)
            break;
        //       printf("(%d,%d):%d\n",i,j,*(g+i*m+j));
        i = ii;
        j = jj;
        step++;
    }
    //   printf("distance1 = %d\n", *(g+(n-1)*m + m -1) / step);
    return step;
    //printf("distance = %d\n", *(g+(n-1)*m + m -1) / step);
}

/*
 *  获取两个特征矢量之间的距离
    参数
    frm_ftr1    特征矢量1
    frm_ftr2    特征矢量2
    返回值
    dis         矢量距离
*/
uint32_t get_dis(int16_t *frm_ftr1, int16_t *frm_ftr2)
{
    uint8_t i;

#if 0
#if 1
    uint32_t dis;
    s32 dif;    //两矢量相同维度上的差值

    dis = 0;
    for (i = 0; i < mfcc_num; i++) {
        //USART1_printf("dis=%d ",dis);
        dif = frm_ftr1[i]-frm_ftr2[i];
        dis += (dif*dif);
    }
    //USART1_printf("dis=%d ",dis);
    dis = sqrtf(dis);
    //USART1_printf("%d\r\n",dis);
    return dis;
#else
    uint32_t dis;
    s32 dif;
    s32 avr;

    dis = 0;
    for (i = 0; i < mfcc_num; i++) {
        //USART1_printf("dis=%d ",dis);
        avr = (frm_ftr1[i]+frm_ftr2[i]) / 2;
        dif = sqrtf((frm_ftr1[i] - avr) * (frm_ftr1[i] - avr) + (frm_ftr2[i] - avr) * (frm_ftr2[i] - avr));

        dif = (frm_ftr1[i]-frm_ftr2[i]) * 100 / dif;

        dis += (dif*dif);
    }
    //USART1_printf("dis=%d ",dis);
    dis = sqrtf(dis);
    //USART1_printf("%d\r\n",dis);
    return dis;
#endif
#else
#if 1
    int64_t dif = 0;
    int64_t dif_a = 0;
    int64_t dif_b = 0;
    int32_t dif_r = 0;

    for (i = 0; i < mfcc_num; i++)
    {
        dif += (frm_ftr1[i] * frm_ftr2[i]);

        dif_a += (frm_ftr1[i] * frm_ftr1[i]);
        dif_b += (frm_ftr2[i] * frm_ftr2[i]);
    }
    dif_r = 1000 - (dif * 1000 / sqrt(dif_a * dif_b));

    return dif_r;
#else
    int64_t dif = 0;
    int64_t dif_a = 0;
    int64_t dif_b = 0;
    int32_t dif_r = 0;

    for (i = 0; i < mfcc_num; i++)
    {
        dif += ((frm_ftr1[i] - avr_mdl[i]) * (frm_ftr2[i] - avr_in[i]));
        dif_a += ((frm_ftr1[i] - avr_mdl[i]) * (frm_ftr1[i] - avr_mdl[i]));
        dif_b += ((frm_ftr2[i] - avr_in[i]) * (frm_ftr2[i] - avr_in[i]));
    }
    dif_r = 1000 - (dif * 1000 / (sqrt(dif_a * dif_b)));

    return dif_r;
#endif

#endif
}

//平行四边形两外两顶点 X坐标值
static uint16_t X1;          //上边交点
static uint16_t X2;          //下边交点
static int in_frm_num;  //输入特征帧数
static int mdl_frm_num; //特征模板帧数

#define ins 0
#define outs 1

/*
 *  范围控制

*/
uint8_t dtw_limit(uint16_t x, uint16_t y)
{
    if (x < X1)
    {
        if (y >= ((2 * x) + 2))
            return outs;
    }
    else
    {
        if ((2 * y + in_frm_num - 2 * mdl_frm_num) >= (x + 4))
            return outs;
    }

    if (x < X2)
    {
        if ((2 * y + 2) <= x)
            return outs;
    }
    else
    {
        if ((y + 4) <= (2 * x + mdl_frm_num - 2 * in_frm_num))
            return outs;
    }

    return ins;
}

#include <stdlib.h>
int *dtw_mem_init(void)
{
    if (pdtw_data != NULL)
    {
        return pdtw_data;
    }
    else
    {
        pdtw_data = (int *)malloc(vv_frm_max * vv_frm_max * sizeof(int));
        return pdtw_data;
    }
    
}

/*
 *  DTW 动态时间规整
    参数
    ftr_in  :输入特征值
    ftr_mdl :特征模版
    返回值
    dis     :累计匹配距离
*/
uint32_t dtw(v_ftr_tag *ftr_in, v_ftr_tag *frt_mdl)
{
    uint32_t dis;
    //  uint16_t x, y;
    uint16_t step;
    int16_t *in;
    int16_t *mdl;
    //  uint32_t d_right_up, right, right_up; //up,
    //  uint32_t min;
    int i, j;
    dtw_mem_init();
    in_frm_num = ftr_in->frm_num;
    mdl_frm_num = frt_mdl->frm_num;

    if ((in_frm_num > (mdl_frm_num * 3)) || ((3 * in_frm_num) < mdl_frm_num))
    {
        //USART1_printf("in_frm_num=%d mdl_frm_num=%d\r\n", in_frm_num,mdl_frm_num);
        return dis_err;
    }
    else
    {
        // 计算约束平行四边形顶点值
        X1 = (2 * mdl_frm_num - in_frm_num) / 3;
        X2 = (4 * in_frm_num - 2 * mdl_frm_num) / 3;
        in = ftr_in->mfcc_dat;
        mdl = frt_mdl->mfcc_dat;
#if 0
        for (j = 0; j < mfcc_num; j++) {
            avr_in[j] = 0;
            avr_mdl[j] = 0;
        }

        for (j = 0; j < mfcc_num; j++)
            for (i = 0; i < in_frm_num; i++)
                avr_in[j] += in[mfcc_num * i + j];
        for (j = 0; j < mfcc_num; j++)
            avr_in[j] = avr_in[j] / in_frm_num;
//
        for (j = 0; j < mfcc_num; j++)
            for (i = 0; i < mdl_frm_num; i++)
                avr_mdl[j] += mdl[mfcc_num * i + j];
        for (j = 0; j < mfcc_num; j++)
            avr_mdl[j] = avr_mdl[j] / mdl_frm_num;

        dis = get_dis(in, mdl);
        x = 1;
        y = 1;
        step = 1;
#endif
#if 0
        for (i = 0; i < in_frm_num; i++) {
            for (j = 0; j < mdl_frm_num; j++) {
                printf("%d,", get_dis(mdl, in));
                mdl += mfcc_num;
            }
            mdl = frt_mdl->mfcc_dat;
            in += mfcc_num;
            printf("\n");
        }
        in = ftr_in->mfcc_dat;
        mdl = frt_mdl->mfcc_dat;
#endif
        for (i = 0; i < in_frm_num; i++)
        {
            for (j = 0; j < mdl_frm_num; j++)
            {
                //dtw_data[i][j] = get_dis(in + (i * mfcc_num), mdl + (j * mfcc_num));
                //dtw_data[i][j] = get_dis(in, mdl);
                *(pdtw_data + i * mdl_frm_num + j) = get_dis(in, mdl);
                // *(dtw_data + i * mdl_frm_num + j) = get_dis(in, mdl);
                //  printf("%d,", dtw_data[i*mdl_frm_num+j]);
                mdl += mfcc_num;
            }
            //printf("\n");
            mdl = frt_mdl->mfcc_dat;
            in += mfcc_num;
        }
        gArray(pdtw_data, in_frm_num, mdl_frm_num, *g, *pOritation);
        // gArray(dtw_data, in_frm_num, mdl_frm_num, *g, *pOritation);
        step = printPath(*pOritation, in_frm_num, mdl_frm_num, *g);
        //printf("step=%d\r\n",step);
        dis = *((int *)g + (in_frm_num - 1) * mdl_frm_num + mdl_frm_num - 1);
        //      printf("dis=%d step=%d dis/step=%d\r\n",dis,step,dis/step);
    }
    return (dis / step); //步长归一化
}

void get_mean(int16_t *frm_ftr1, int16_t *frm_ftr2, int16_t *mean)
{
    uint8_t i;

    for (i = 0; i < mfcc_num; i++)
    {
        mean[i] = (frm_ftr1[i] + frm_ftr2[i]) / 2;
        //  printf("x=%d y=%d ", frm_ftr1[i], frm_ftr2[i]);
        //  printf("mean=%d\r\n", mean[i]);
    }
}

/*
 *  从两特征矢量获取特征模板
    参数
    ftr_in1 :输入特征值
    ftr_in2 :输入特征值
    ftr_mdl :特征模版
    返回值
    dis     :累计匹配距离
*/

uint32_t get_mdl(v_ftr_tag *ftr_in1, v_ftr_tag *ftr_in2, v_ftr_tag *ftr_mdl)
{
    uint32_t dis;
    uint16_t x, y;
    uint16_t step;
    int16_t *in1;
    int16_t *in2;
    int16_t *mdl;
    uint32_t right, right_up, d_right_up; //up,
    uint32_t min;

    in_frm_num = ftr_in1->frm_num;
    mdl_frm_num = ftr_in2->frm_num;

    if ((in_frm_num > (mdl_frm_num * 2)) || ((2 * in_frm_num) < mdl_frm_num))
    {
        printf("in_frm_num= %d, mdl_frm_num= %d\n", in_frm_num, mdl_frm_num);
        return dis_err;
    }
    else
    {
        // 计算约束平行四边形顶点值
        X1 = (2 * mdl_frm_num - in_frm_num) / 3;
        X2 = (4 * in_frm_num - 2 * mdl_frm_num) / 3;
        in1 = ftr_in1->mfcc_dat;
        in2 = ftr_in2->mfcc_dat;
        mdl = ftr_mdl->mfcc_dat;

        dis = get_dis(in1, in2);
        get_mean(in1, in2, mdl);
        x = 1;
        y = 1;
        step = 1;
        do
        {
            //up = (dtw_limit(x, y+1) == ins)?get_dis(in2+mfcc_num, in1):dis_err;
            d_right_up = (dtw_limit(x + 1, y + 2) == ins) ? get_dis(in2 + mfcc_num + mfcc_num, in1 + mfcc_num) : dis_err;
            right = (dtw_limit(x + 1, y) == ins) ? get_dis(in2, in1 + mfcc_num) : dis_err;
            right_up = (dtw_limit(x + 1, y + 1) == ins) ? get_dis(in2 + mfcc_num, in1 + mfcc_num) : dis_err;

            min = right_up;
            if (min > right)
                min = right;

            if (min > d_right_up)
                min = d_right_up;

            dis += min;

            if (min == right_up)
            {
                in1 += mfcc_num;
                x++;
                in2 += mfcc_num;
                y++;
            }
            else if (min == d_right_up)
            {
                //in2 += mfcc_num;
                //y++;
                in2 = in2 + mfcc_num + mfcc_num;
                y += 2;
                in1 += mfcc_num;
                x++;
            }
            else
            {
                in1 += mfcc_num;
                x++;
            }
            step++;

            mdl += mfcc_num;
            get_mean(in1, in2, mdl);

            //  printf("x=%d y=%d\r\n", x, y);
        } while ((x < in_frm_num) && (y < mdl_frm_num));
        printf("step=%d\r\n", step);
        ftr_mdl->frm_num = step;
    }
    return (dis / step); //步长归一化
}
